#include "RrdGlcd.h"

#include "platform_memory.h"
#include "StreamOutputPool.h"

static const uint8_t font5x8[] = {
    // 5x8 font each byte is consecutive x bits left aligned then each subsequent byte is Y 8 bytes per character
    // TODO probably only need someof these characters not all 256
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, 0x70,0xF8,0xA8,0xF8,0xD8,0x88,0x70,0x00, // 0x00, 0x01
    0x70,0xF8,0xA8,0xF8,0x88,0xD8,0x70,0x00, 0x00,0x50,0xF8,0xF8,0xF8,0x70,0x20,0x00, // 0x02, 0x03
    0x00,0x20,0x70,0xF8,0xF8,0x70,0x20,0x00, 0x70,0x50,0xF8,0xA8,0xF8,0x20,0x70,0x00, // 0x04, 0x05
    0x20,0x70,0xF8,0xF8,0xF8,0x20,0x70,0x00, 0x00,0x00,0x20,0x70,0x70,0x20,0x00,0x00, // 0x06, 0x07
    0xF8,0xF8,0xD8,0x88,0x88,0xD8,0xF8,0xF8, 0x00,0x00,0x20,0x50,0x50,0x20,0x00,0x00, // 0x08, 0x09
    0xF8,0xF8,0xD8,0xA8,0xA8,0xD8,0xF8,0xF8, 0x00,0x38,0x18,0x68,0xA0,0xA0,0x40,0x00, // 0x0A, 0x0B
    0x70,0x88,0x88,0x70,0x20,0xF8,0x20,0x00, 0x78,0x48,0x78,0x40,0x40,0x40,0xC0,0x00, // 0x0C, 0x0D
    0x78,0x48,0x78,0x48,0x48,0x58,0xC0,0x00, 0x20,0xA8,0x70,0xD8,0xD8,0x70,0xA8,0x20, // 0x0E, 0x0F
    0x80,0xC0,0xF0,0xF8,0xF0,0xC0,0x80,0x00, 0x08,0x18,0x78,0xF8,0x78,0x18,0x08,0x00, // 0x10, 0x11
    0x20,0x70,0xA8,0x20,0xA8,0x70,0x20,0x00, 0xD8,0xD8,0xD8,0xD8,0xD8,0x00,0xD8,0x00, // 0x12, 0x13
    0x78,0xA8,0xA8,0x68,0x28,0x28,0x28,0x00, 0x30,0x48,0x50,0x28,0x10,0x48,0x48,0x30, // 0x14, 0x15
    0x00,0x00,0x00,0x00,0x00,0xF8,0xF8,0x00, 0x20,0x70,0xA8,0x20,0xA8,0x70,0x20,0xF8, // 0x16, 0x17
    0x00,0x20,0x70,0xA8,0x20,0x20,0x20,0x00, 0x00,0x20,0x20,0x20,0xA8,0x70,0x20,0x00, // 0x18, 0x19
    0x00,0x20,0x10,0xF8,0x10,0x20,0x00,0x00, 0x00,0x20,0x40,0xF8,0x40,0x20,0x00,0x00, // 0x1A, 0x1B
    0x00,0x80,0x80,0x80,0xF8,0x00,0x00,0x00, 0x00,0x50,0xF8,0xF8,0x50,0x00,0x00,0x00, // 0x1C, 0x1D
    0x00,0x20,0x20,0x70,0xF8,0xF8,0x00,0x00, 0x00,0xF8,0xF8,0x70,0x20,0x20,0x00,0x00, // 0x1E, 0x1F
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, 0x20,0x20,0x20,0x20,0x20,0x00,0x20,0x00, // 0x20, 0x21
    0x50,0x50,0x50,0x00,0x00,0x00,0x00,0x00, 0x50,0x50,0xF8,0x50,0xF8,0x50,0x50,0x00, // 0x22, 0x23
    0x20,0x78,0xA0,0x70,0x28,0xF0,0x20,0x00, 0xC0,0xC8,0x10,0x20,0x40,0x98,0x18,0x00, // 0x24, 0x25
    0x40,0xA0,0xA0,0x40,0xA8,0x90,0x68,0x00, 0x30,0x30,0x20,0x40,0x00,0x00,0x00,0x00, // 0x26, 0x27
    0x10,0x20,0x40,0x40,0x40,0x20,0x10,0x00, 0x40,0x20,0x10,0x10,0x10,0x20,0x40,0x00, // 0x28, 0x29
    0x20,0xA8,0x70,0xF8,0x70,0xA8,0x20,0x00, 0x00,0x20,0x20,0xF8,0x20,0x20,0x00,0x00, // 0x2A, 0x2B
    0x00,0x00,0x00,0x00,0x30,0x30,0x20,0x40, 0x00,0x00,0x00,0xF8,0x00,0x00,0x00,0x00, // 0x2C, 0x2D
    0x00,0x00,0x00,0x00,0x00,0x30,0x30,0x00, 0x00,0x08,0x10,0x20,0x40,0x80,0x00,0x00, // 0x2E, 0x2F
    0x70,0x88,0x98,0xA8,0xC8,0x88,0x70,0x00, 0x20,0x60,0x20,0x20,0x20,0x20,0x70,0x00, // 0x30, 0x31
    0x70,0x88,0x08,0x70,0x80,0x80,0xF8,0x00, 0xF8,0x08,0x10,0x30,0x08,0x88,0x70,0x00, // 0x32, 0x33
    0x10,0x30,0x50,0x90,0xF8,0x10,0x10,0x00, 0xF8,0x80,0xF0,0x08,0x08,0x88,0x70,0x00, // 0x34, 0x35
    0x38,0x40,0x80,0xF0,0x88,0x88,0x70,0x00, 0xF8,0x08,0x08,0x10,0x20,0x40,0x80,0x00, // 0x36, 0x37
    0x70,0x88,0x88,0x70,0x88,0x88,0x70,0x00, 0x70,0x88,0x88,0x78,0x08,0x10,0xE0,0x00, // 0x38, 0x39
    0x00,0x00,0x20,0x00,0x20,0x00,0x00,0x00, 0x00,0x00,0x20,0x00,0x20,0x20,0x40,0x00, // 0x3A, 0x3B
    0x08,0x10,0x20,0x40,0x20,0x10,0x08,0x00, 0x00,0x00,0xF8,0x00,0xF8,0x00,0x00,0x00, // 0x3C, 0x3D
    0x40,0x20,0x10,0x08,0x10,0x20,0x40,0x00, 0x70,0x88,0x08,0x30,0x20,0x00,0x20,0x00, // 0x3E, 0x3F
    0x70,0x88,0xA8,0xB8,0xB0,0x80,0x78,0x00, 0x20,0x50,0x88,0x88,0xF8,0x88,0x88,0x00, // 0x40, 0x41
    0xF0,0x88,0x88,0xF0,0x88,0x88,0xF0,0x00, 0x70,0x88,0x80,0x80,0x80,0x88,0x70,0x00, // 0x42, 0x43
    0xF0,0x88,0x88,0x88,0x88,0x88,0xF0,0x00, 0xF8,0x80,0x80,0xF0,0x80,0x80,0xF8,0x00, // 0x44, 0x45
    0xF8,0x80,0x80,0xF0,0x80,0x80,0x80,0x00, 0x78,0x88,0x80,0x80,0x98,0x88,0x78,0x00, // 0x46, 0x47
    0x88,0x88,0x88,0xF8,0x88,0x88,0x88,0x00, 0x70,0x20,0x20,0x20,0x20,0x20,0x70,0x00, // 0x48, 0x49
    0x38,0x10,0x10,0x10,0x10,0x90,0x60,0x00, 0x88,0x90,0xA0,0xC0,0xA0,0x90,0x88,0x00, // 0x4A, 0x4B
    0x80,0x80,0x80,0x80,0x80,0x80,0xF8,0x00, 0x88,0xD8,0xA8,0xA8,0xA8,0x88,0x88,0x00, // 0x4C, 0x4D
    0x88,0x88,0xC8,0xA8,0x98,0x88,0x88,0x00, 0x70,0x88,0x88,0x88,0x88,0x88,0x70,0x00, // 0x4E, 0x4F
    0xF0,0x88,0x88,0xF0,0x80,0x80,0x80,0x00, 0x70,0x88,0x88,0x88,0xA8,0x90,0x68,0x00, // 0x50, 0x51
    0xF0,0x88,0x88,0xF0,0xA0,0x90,0x88,0x00, 0x70,0x88,0x80,0x70,0x08,0x88,0x70,0x00, // 0x52, 0x53
    0xF8,0xA8,0x20,0x20,0x20,0x20,0x20,0x00, 0x88,0x88,0x88,0x88,0x88,0x88,0x70,0x00, // 0x54, 0x55
    0x88,0x88,0x88,0x88,0x88,0x50,0x20,0x00, 0x88,0x88,0x88,0xA8,0xA8,0xA8,0x50,0x00, // 0x56, 0x57
    0x88,0x88,0x50,0x20,0x50,0x88,0x88,0x00, 0x88,0x88,0x50,0x20,0x20,0x20,0x20,0x00, // 0x58, 0x59
    0xF8,0x08,0x10,0x70,0x40,0x80,0xF8,0x00, 0x78,0x40,0x40,0x40,0x40,0x40,0x78,0x00, // 0x5A, 0x5B
    0x00,0x80,0x40,0x20,0x10,0x08,0x00,0x00, 0x78,0x08,0x08,0x08,0x08,0x08,0x78,0x00, // 0x5C, 0x5D
    0x20,0x50,0x88,0x00,0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x00, // 0x5E, 0x5F
    0x60,0x60,0x20,0x10,0x00,0x00,0x00,0x00, 0x00,0x00,0x60,0x10,0x70,0x90,0x78,0x00, // 0x60, 0x61
    0x80,0x80,0xB0,0xC8,0x88,0xC8,0xB0,0x00, 0x00,0x00,0x70,0x88,0x80,0x88,0x70,0x00, // 0x62, 0x63
    0x08,0x08,0x68,0x98,0x88,0x98,0x68,0x00, 0x00,0x00,0x70,0x88,0xF8,0x80,0x70,0x00, // 0x64, 0x65
    0x10,0x28,0x20,0x70,0x20,0x20,0x20,0x00, 0x00,0x00,0x70,0x98,0x98,0x68,0x08,0x70, // 0x66, 0x67
    0x80,0x80,0xB0,0xC8,0x88,0x88,0x88,0x00, 0x20,0x00,0x60,0x20,0x20,0x20,0x70,0x00, // 0x68, 0x69
    0x10,0x00,0x10,0x10,0x10,0x90,0x60,0x00, 0x80,0x80,0x90,0xA0,0xC0,0xA0,0x90,0x00, // 0x6A, 0x6B
    0x60,0x20,0x20,0x20,0x20,0x20,0x70,0x00, 0x00,0x00,0xD0,0xA8,0xA8,0xA8,0xA8,0x00, // 0x6C, 0x6D
    0x00,0x00,0xB0,0xC8,0x88,0x88,0x88,0x00, 0x00,0x00,0x70,0x88,0x88,0x88,0x70,0x00, // 0x6E, 0x6F
    0x00,0x00,0xB0,0xC8,0xC8,0xB0,0x80,0x80, 0x00,0x00,0x68,0x98,0x98,0x68,0x08,0x08, // 0x70, 0x71
    0x00,0x00,0xB0,0xC8,0x80,0x80,0x80,0x00, 0x00,0x00,0x78,0x80,0x70,0x08,0xF0,0x00, // 0x72, 0x73
    0x20,0x20,0xF8,0x20,0x20,0x28,0x10,0x00, 0x00,0x00,0x88,0x88,0x88,0x98,0x68,0x00, // 0x74, 0x75
    0x00,0x00,0x88,0x88,0x88,0x50,0x20,0x00, 0x00,0x00,0x88,0x88,0xA8,0xA8,0x50,0x00, // 0x76, 0x77
    0x00,0x00,0x88,0x50,0x20,0x50,0x88,0x00, 0x00,0x00,0x88,0x88,0x78,0x08,0x88,0x70, // 0x78, 0x79
    0x00,0x00,0xF8,0x10,0x20,0x40,0xF8,0x00, 0x10,0x20,0x20,0x40,0x20,0x20,0x10,0x00, // 0x7A, 0x7B
    0x20,0x20,0x20,0x00,0x20,0x20,0x20,0x00, 0x40,0x20,0x20,0x10,0x20,0x20,0x40,0x00, // 0x7C, 0x7D
    0x80,0xC0,0xF0,0xF8,0xF0,0xC0,0x80,0x00, 0x20,0x70,0xD8,0x88,0x88,0xF8,0x00,0x00, // 0x7E, 0x7F // 7e ~ = 0x40,0xA8,0x10,0x00,0x00,0x00,0x00,0x00
    0x70,0x88,0x80,0x80,0x88,0x70,0x10,0x60, 0x00,0x88,0x00,0x88,0x88,0x98,0x68,0x00, // 0x80, 0x81
    0x18,0x00,0x70,0x88,0xF8,0x80,0x78,0x00, 0xF8,0x00,0x60,0x10,0x70,0x90,0x78,0x00, // 0x82, 0x83
    0x88,0x00,0x60,0x10,0x70,0x90,0x78,0x00, 0xC0,0x00,0x60,0x10,0x70,0x90,0x78,0x00, // 0x84, 0x85
    0x30,0x00,0x60,0x10,0x70,0x90,0x78,0x00, 0x00,0x78,0xC0,0xC0,0x78,0x10,0x30,0x00, // 0x86, 0x87
    0xF8,0x00,0x70,0x88,0xF8,0x80,0x78,0x00, 0x88,0x00,0x70,0x88,0xF8,0x80,0x78,0x00, // 0x88, 0x89
    0xC0,0x00,0x70,0x88,0xF8,0x80,0x78,0x00, 0x28,0x00,0x30,0x10,0x10,0x10,0x38,0x00, // 0x8A, 0x8B
    0x30,0x48,0x30,0x10,0x10,0x10,0x38,0x00, 0x60,0x00,0x30,0x10,0x10,0x10,0x38,0x00, // 0x8C, 0x8D
    0x50,0x00,0x20,0x50,0x88,0xF8,0x88,0x88, 0x20,0x00,0x20,0x50,0x88,0xF8,0x88,0x88, // 0x8E, 0x8F
    0x30,0x00,0xF0,0x80,0xE0,0x80,0xF0,0x00, 0x00,0x00,0x78,0x10,0x78,0x90,0x78,0x00, // 0x90, 0x91
    0x38,0x50,0x90,0xF8,0x90,0x90,0x98,0x00, 0x70,0x88,0x00,0x70,0x88,0x88,0x70,0x00, // 0x92, 0x93
    0x00,0x88,0x00,0x70,0x88,0x88,0x70,0x00, 0x00,0xC0,0x00,0x70,0x88,0x88,0x70,0x00, // 0x94, 0x95
    0x70,0x88,0x00,0x88,0x88,0x98,0x68,0x00, 0x00,0xC0,0x00,0x88,0x88,0x98,0x68,0x00, // 0x96, 0x97
    0x48,0x00,0x48,0x48,0x48,0x38,0x08,0x70, 0x88,0x00,0x70,0x88,0x88,0x88,0x70,0x00, // 0x98, 0x99
    0x88,0x00,0x88,0x88,0x88,0x88,0x70,0x00, 0x20,0x20,0xF8,0xA0,0xA0,0xF8,0x20,0x20, // 0x9A, 0x9B
    0x30,0x58,0x48,0xE0,0x40,0x48,0xF8,0x00, 0xD8,0xD8,0x70,0xF8,0x20,0xF8,0x20,0x20, // 0x9C, 0x9D
    0xE0,0x90,0x90,0xE0,0x90,0xB8,0x90,0x90, 0x18,0x28,0x20,0x70,0x20,0x20,0xA0,0xC0, // 0x9E, 0x9F
    0x18,0x00,0x60,0x10,0x70,0x90,0x78,0x00, 0x18,0x00,0x30,0x10,0x10,0x10,0x38,0x00, // 0xA0, 0xA1
    0x00,0x18,0x00,0x70,0x88,0x88,0x70,0x00, 0x00,0x18,0x00,0x88,0x88,0x98,0x68,0x00, // 0xA2, 0xA3
    0x00,0x78,0x00,0x70,0x48,0x48,0x48,0x00, 0xF8,0x00,0xC8,0xE8,0xB8,0x98,0x88,0x00, // 0xA4, 0xA5
    0x70,0x90,0x90,0x78,0x00,0xF8,0x00,0x00, 0x70,0x88,0x88,0x70,0x00,0xF8,0x00,0x00, // 0xA6, 0xA7
    0x20,0x00,0x20,0x60,0x80,0x88,0x70,0x00, 0x00,0x00,0x00,0xF8,0x80,0x80,0x00,0x00, // 0xA8, 0xA9
    0x00,0x00,0x00,0xF8,0x08,0x08,0x00,0x00, 0x80,0x88,0x90,0xB8,0x48,0x98,0x20,0x38, // 0xAA, 0xAB
    0x80,0x88,0x90,0xA8,0x58,0xB8,0x08,0x08, 0x20,0x20,0x00,0x20,0x20,0x20,0x20,0x00, // 0xAC, 0xAD
    0x00,0x28,0x50,0xA0,0x50,0x28,0x00,0x00, 0x00,0xA0,0x50,0x28,0x50,0xA0,0x00,0x00, // 0xAE, 0xAF
    0x20,0x88,0x20,0x88,0x20,0x88,0x20,0x88, 0x50,0xA8,0x50,0xA8,0x50,0xA8,0x50,0xA8, // 0xB0, 0xB1
    0xA8,0x50,0xA8,0x50,0xA8,0x50,0xA8,0x50, 0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10, // 0xB2, 0xB3
    0x10,0x10,0x10,0x10,0xF0,0x10,0x10,0x10, 0x10,0x10,0xF0,0x10,0xF0,0x10,0x10,0x10, // 0xB4, 0xB5
    0x28,0x28,0x28,0x28,0xE8,0x28,0x28,0x28, 0x00,0x00,0x00,0x00,0xF8,0x28,0x28,0x28, // 0xB6, 0xB7
    0x00,0x00,0xF0,0x10,0xF0,0x10,0x10,0x10, 0x28,0x28,0xE8,0x08,0xE8,0x28,0x28,0x28, // 0xB8, 0xB9
    0x28,0x28,0x28,0x28,0x28,0x28,0x28,0x28, 0x00,0x00,0xF8,0x08,0xE8,0x28,0x28,0x28, // 0xBA, 0xBB
    0x28,0x28,0xE8,0x08,0xF8,0x00,0x00,0x00, 0x28,0x28,0x28,0x28,0xF8,0x00,0x00,0x00, // 0xBC, 0xBD
    0x10,0x10,0xF0,0x10,0xF0,0x00,0x00,0x00, 0x00,0x00,0x00,0x00,0xF0,0x10,0x10,0x10, // 0xBE, 0xBF
    0x10,0x10,0x10,0x10,0x18,0x00,0x00,0x00, 0x10,0x10,0x10,0x10,0xF8,0x00,0x00,0x00, // 0xC0, 0xC1
    0x00,0x00,0x00,0x00,0xF8,0x10,0x10,0x10, 0x10,0x10,0x10,0x10,0x18,0x10,0x10,0x10, // 0xC2, 0xC3
    0x00,0x00,0x00,0x00,0xF8,0x00,0x00,0x00, 0x10,0x10,0x10,0x10,0xF8,0x10,0x10,0x10, // 0xC4, 0xC5
    0x10,0x10,0x18,0x10,0x18,0x10,0x10,0x10, 0x28,0x28,0x28,0x28,0x28,0x28,0x28,0x28, // 0xC6, 0xC7
    0x28,0x28,0x28,0x20,0x38,0x00,0x00,0x00, 0x00,0x00,0x38,0x20,0x28,0x28,0x28,0x28, // 0xC8, 0xC9
    0x28,0x28,0xE8,0x00,0xF8,0x00,0x00,0x00, 0x00,0x00,0xF8,0x00,0xE8,0x28,0x28,0x28, // 0xCA, 0xCB
    0x28,0x28,0x28,0x20,0x28,0x28,0x28,0x28, 0x00,0x00,0xF8,0x00,0xF8,0x00,0x00,0x00, // 0xCC, 0xCD
    0x28,0x28,0xE8,0x00,0xE8,0x28,0x28,0x28, 0x10,0x10,0xF8,0x00,0xF8,0x00,0x00,0x00, // 0xCE, 0xCF
    0x28,0x28,0x28,0x28,0xF8,0x00,0x00,0x00, 0x00,0x00,0xF8,0x00,0xF8,0x10,0x10,0x10, // 0xD0, 0xD1
    0x00,0x00,0x00,0x00,0xF8,0x28,0x28,0x28, 0x28,0x28,0x28,0x28,0x38,0x00,0x00,0x00, // 0xD2, 0xD3
    0x10,0x10,0x18,0x10,0x18,0x00,0x00,0x00, 0x00,0x00,0x18,0x10,0x18,0x10,0x10,0x10, // 0xD4, 0xD5
    0x00,0x00,0x00,0x00,0x38,0x28,0x28,0x28, 0x28,0x28,0x28,0x28,0xF8,0x28,0x28,0x28, // 0xD6, 0xD7
    0x10,0x10,0xF8,0x10,0xF8,0x10,0x10,0x10, 0x10,0x10,0x10,0x10,0xF0,0x00,0x00,0x00, // 0xD8, 0xD9
    0x00,0x00,0x00,0x00,0x18,0x10,0x10,0x10, 0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8, // 0xDA, 0xDB
    0x00,0x00,0x00,0x00,0xF8,0xF8,0xF8,0xF8, 0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,0xE0, // 0xDC, 0xDD
    0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18, 0xF8,0xF8,0xF8,0xF8,0x00,0x00,0x00,0x00, // 0xDE, 0xDF
    0x00,0x00,0x68,0x90,0x90,0x90,0x68,0x00, 0x00,0x70,0x98,0xF0,0x98,0xF0,0x80,0x00, // 0xE0, 0xE1
    0x00,0xF8,0x98,0x80,0x80,0x80,0x80,0x00, 0x00,0xF8,0x50,0x50,0x50,0x50,0x50,0x00, // 0xE2, 0xE3
    0xF8,0x88,0x40,0x20,0x40,0x88,0xF8,0x00, 0x00,0x00,0x78,0x90,0x90,0x90,0x60,0x00, // 0xE4, 0xE5
    0x00,0x50,0x50,0x50,0x50,0x68,0xC0,0x00, 0x00,0xF8,0xA0,0x20,0x20,0x20,0x20,0x00, // 0xE6, 0xE7
    0xF8,0x20,0x70,0x88,0x88,0x70,0x20,0xF8, 0x20,0x50,0x88,0xF8,0x88,0x50,0x20,0x00, // 0xE8, 0xE9
    0x20,0x50,0x88,0x88,0x50,0x50,0xD8,0x00, 0x30,0x40,0x30,0x70,0x88,0x88,0x70,0x00, // 0xEA, 0xEB
    0x00,0x00,0x00,0x70,0xA8,0xA8,0x70,0x00, 0x08,0x70,0x98,0xA8,0xA8,0xC8,0x70,0x80, // 0xEC, 0xED
    0x70,0x80,0x80,0xF0,0x80,0x80,0x70,0x00, 0x70,0x88,0x88,0x88,0x88,0x88,0x88,0x00, // 0xEE, 0xEF
    0x00,0xF8,0x00,0xF8,0x00,0xF8,0x00,0x00, 0x20,0x20,0xF8,0x20,0x20,0x00,0xF8,0x00, // 0xF0, 0xF1
    0x40,0x20,0x10,0x20,0x40,0x00,0xF8,0x00, 0x10,0x20,0x40,0x20,0x10,0x00,0xF8,0x00, // 0xF2, 0xF3
    0x38,0x28,0x20,0x20,0x20,0x20,0x20,0x20, 0x20,0x20,0x20,0x20,0x20,0xA0,0xA0,0xE0, // 0xF4, 0xF5
    0x30,0x30,0x00,0xF8,0x00,0x30,0x30,0x00, 0x00,0xE8,0xB8,0x00,0xE8,0xB8,0x00,0x00, // 0xF6, 0xF7
    0x70,0xD8,0xD8,0x70,0x00,0x00,0x00,0x00, 0x50,0x20,0x00,0x00,0x00,0x00,0x08,0x00, // 0xF8, 0xF9
    0x00,0x00,0x00,0x00,0x30,0x00,0x00,0x00, 0x38,0x20,0x20,0x20,0xA0,0xA0,0x60,0x20, // 0xFA, 0xFB
    0x70,0x48,0x48,0x48,0x48,0x00,0x00,0x00, 0x70,0x18,0x30,0x60,0x78,0x00,0x00,0x00, // 0xFC, 0xFD
    0x00,0x00,0x78,0x78,0x78,0x78,0x00,0x00
};

#define ST7920_CS()              {cs.set(1);wait_us(10);}
#define ST7920_NCS()             {cs.set(0);wait_us(10);}
#define ST7920_WRITE_BYTE(a)     {this->spi->write((a)&0xf0);this->spi->write((a)<<4);wait_us(10);}
#define ST7920_WRITE_BYTES(p,l)  {uint8_t i;for(i=0;i<l;i++){this->spi->write(*p&0xf0);this->spi->write(*p<<4);p++;} wait_us(10); }
#define ST7920_SET_CMD()         {this->spi->write(0xf8);wait_us(10);}
#define ST7920_SET_DAT()         {this->spi->write(0xfa);wait_us(10);}
#define PAGE_HEIGHT 32  //512 byte framebuffer
#define WIDTH 128
#define HEIGHT 64
#define FB_SIZE WIDTH*HEIGHT/8

RrdGlcd::RrdGlcd(int spi_channel, Pin cs) {
    PinName mosi, miso, sclk;
    if(spi_channel == 0) {
        mosi = P0_18; miso = P0_17; sclk = P0_15;
    } else if(spi_channel == 1) {
        mosi = P0_9; miso = P0_8; sclk = P0_7;
    } else {
        mosi = P0_18; miso = P0_17; sclk = P0_15;
    }

    this->spi = new mbed::SPI(mosi, miso, sclk);

    //chip select
    this->cs= cs;
    this->cs.set(0);
    fb= (uint8_t *)AHB0.alloc(FB_SIZE); // grab some memoery from USB_RAM
    if(fb == NULL) {
        THEKERNEL->streams->printf("Not enough memory available for frame buffer");
    }
    inited= false;
    dirty= false;
}

RrdGlcd::~RrdGlcd() {
    delete this->spi;
    AHB0.dealloc(fb);
}

void RrdGlcd::setFrequency(int freq) {
       this->spi->frequency(freq);
}

void RrdGlcd::initDisplay() {
    if(fb == NULL) return;
    ST7920_CS();
    clearScreen();  // clear framebuffer
    wait_ms(90);                 //initial delay for boot up
    ST7920_SET_CMD();
    ST7920_WRITE_BYTE(0x08);       //display off, cursor+blink off
    ST7920_WRITE_BYTE(0x01);       //clear CGRAM ram
    wait_ms(10);                 //delay for cgram clear
    ST7920_WRITE_BYTE(0x3E);       //extended mode + gdram active
    for(int y=0;y<HEIGHT/2;y++)        //clear GDRAM
    {
        ST7920_WRITE_BYTE(0x80|y);   //set y
        ST7920_WRITE_BYTE(0x80);     //set x = 0
        ST7920_SET_DAT();
        for(int i=0;i<2*WIDTH/8;i++)     //2x width clears both segments
            ST7920_WRITE_BYTE(0);
        ST7920_SET_CMD();
    }
    ST7920_WRITE_BYTE(0x0C); //display on, cursor+blink off
    ST7920_NCS();
    inited= true;
}

void RrdGlcd::clearScreen() {
    if(fb == NULL) return;
    memset(this->fb, 0, FB_SIZE);
    dirty= true;
}

// render into local screenbuffer
void RrdGlcd::displayString(int row, int col, const char *ptr, int length) {
    for (int i = 0; i < length; ++i) {
        displayChar(row, col, ptr[i]);
        col+=1;
    }
    dirty= true;
}

void RrdGlcd::renderChar(uint8_t *fb, char c, int ox, int oy) {
    if(fb == NULL) return;
    // using the specific font data where x is in one byte and y is in consecutive bytes
    // the x bits are left aligned and right padded
    int i= c*8; // character offset in font array
    int o= ox%8; // where in fb byte does it go
    int a= oy*16 + ox/8; // start address in frame buffer
    int mask= ~0xF8 >> o; // mask off top bits
    int mask2= ~0xF8 << (8-o); // mask off bottom bits
    for(int y=0;y<8;y++) {
        int b= font5x8[i+y]; // get font byte
        fb[a] &= mask; // clear top bits for font
        fb[a] |= (b>>o); // or in the fonts 1 bits
        if(o >= 4) { // it spans two fb bytes
            fb[a+1] &= mask2; // clear bottom bits for font
            fb[a+1] |= (b<<(8-o)); // or in the fonts 1 bits
        }
        a+=16; // next line
    }
}

void RrdGlcd::displayChar(int row, int col, char c) {
    int x= col*6;
    // if this wraps the line ignore it
    if(x+6 > WIDTH) return;

    // convert row/column into y and x pixel positions based on font size
    renderChar(this->fb, c, x, row*8);
}

void RrdGlcd::renderGlyph(int xp, int yp, const uint8_t *g, int pixelWidth, int pixelHeight) {
    if(fb == NULL) return;
    // NOTE the source is expected to be byte aligned and the exact number of pixels
    // TODO need to optimize by copying bytes instead of pixels...
    int xf= xp%8;
    int rf= pixelWidth%8;
    int a= yp*16 + xp/8; // start address in frame buffer
    const uint8_t *src= g;
    if(xf == 0) {
        // If xp is on a byte boundary simply memcpy each line from source to dest
        uint8_t *dest= &fb[a];
        int n= pixelWidth/8; // bytes per line to copy
        if(rf != 0) n++; // if not a multiple of 8 pixels copy last byte as a byte
        if(n > 0) {
            for(int y=0;y<pixelHeight;y++) {
                memcpy(dest, src, n);
                src += n;
                dest+=16; // next line
            }
        }

        // TODO now handle ragged end if we have one but as we always render left to right we probably don't need to
        // if(rf != 0) {

        // }
        return;
    }

    // if xp is not on a byte boundary we do the slow pixel by pixel copy
    int b= *g++;
    for(int y=0;y<pixelHeight;y++) {
        int m= 0x80;
        for(int x=0;x<pixelWidth;x++) {
            a= (y+yp)*16 + (x+xp)/8;
            int p= 1<<(7-(x+xp)%8);
            if((b & m) != 0){
                fb[a] |= p;
            }else{
                fb[a] &= ~p;
            }
            m= m>>1;
            if(m == 0){
                m= 0x80;
                b= *g++;
            }
        }
    }
}

// copy frame buffer to graphic buffer on display
void RrdGlcd::fillGDRAM(const uint8_t *bitmap) {
    unsigned char i, y;
    for ( i = 0 ; i < 2 ; i++ ) {
        ST7920_CS();
        for ( y = 0 ; y < PAGE_HEIGHT ; y++ ) {
            ST7920_SET_CMD();
            ST7920_WRITE_BYTE(0x80 | y);
            if ( i == 0 ) {
                ST7920_WRITE_BYTE(0x80);
            } else {
                ST7920_WRITE_BYTE(0x80 | 0x08);
            }
            ST7920_SET_DAT();
            ST7920_WRITE_BYTES(bitmap, WIDTH/8); // bitmap gets incremented in this macro
        }
        ST7920_NCS();
    }
}

void RrdGlcd::refresh() {
    if(!inited || !dirty) return;
    fillGDRAM(this->fb);
    dirty= false;
}
